#include "v4l_cpp.h"
// #include "misc_cpp.h"

#define CAMERA_DEVICE_NAME "/dev/video0"

camera_v4l::camera_v4l()
{
	s_sVidData.i32VidFD = 1;
	s_sVidData.i32UseMMap = 0;
	s_sVidData.i32VidFrame = 0;
	s_u64PrevTS = 0;
	device_name = CAMERA_DEVICE_NAME;
	ctrl_id = 0;
}
camera_v4l::camera_v4l(string name)
{
	device_name = name;
	s_sVidData.i32VidFD = 1;
	s_sVidData.i32UseMMap = 0;
	s_sVidData.i32VidFrame = 0;
	
	s_u64PrevTS = 0;
	ctrl_id = 0;
}
ERRCODE  camera_v4l::InitV4LDevice(E_IMAGE_RESOL eImageResol)
{
	int i32VidFD;
	ERRCODE err = ERR_V4L_SUCCESS;
	struct video_capability sVidCap;
	struct video_picture sPict;
	int  i32Width, i32Height;

	i32VidFD = open(device_name.c_str(),O_RDWR);
	if(i32VidFD < 0)
	{
		printf("Can not open video device\n");
		err = ERR_V4L_OPEN_DEV;	
		goto fail;
	}

	
	if (ioctl(i32VidFD, VIDIOCGCAP, &sVidCap) < 0)
	{
		printf("VIDIOCGCAP: %s\n", strerror(errno));
		err = ERR_V4L_VID_GET_CAP;
		goto fail;
	}

	if (!(sVidCap.type & VID_TYPE_CAPTURE))
	{
		printf("Fatal: grab device does not handle capture\n");
		err = ERR_V4L_VID_GRAB_DEV;
		goto fail;
	}


	if(eImageResol == eIMAGE_VGA)
	{
		i32Width = 640;
		i32Height = 480;
	}
	else if (eImageResol == eIMAGE_WQVGA)
	{
		i32Width = 480;
		i32Height = 272;
	}
	else{	//QVGA
		i32Width = 320;
		i32Height = 240;
	}

	ioctl(i32VidFD, VIDIOCGPICT, &sPict);
	sPict.palette = VIDEO_PALETTE_YUV422;
	sPict.depth= 16;

	if(ioctl(i32VidFD, VIDIOCSPICT, &sPict) < 0)
	{
		printf("VIDIOCSPICT: %s\n", strerror(errno));
		err = ERR_V4L_VID_SET_PICT;
		goto fail;
	}
	if (ioctl(i32VidFD, VIDIOCGMBUF, &s_sVidData.sVidMBufs) < 0)
	{
		/* try to use read based access */
		int val;
		printf(" try to use read based access\n");
		s_sVidData.sVidWin.width = i32Width;
		s_sVidData.sVidWin.height = i32Height;
		s_sVidData.sVidWin.x = 0;
		s_sVidData.sVidWin.y = 0;
		s_sVidData.sVidWin.chromakey = -1;
		s_sVidData.sVidWin.flags = 0;
		printf("s_sVidData.sVidWin.width:%d\ns_sVidData.sVidWin.height:%d\n",s_sVidData.sVidWin.width,s_sVidData.sVidWin.height);

		if (ioctl(i32VidFD, VIDIOCSWIN, &s_sVidData.sVidWin) < 0)
		{
			printf("VIDIOCSWIN: %s\n", strerror(errno));
			err = ERR_V4L_VID_SET_WIN;
			goto fail;
		}

		s_sVidData.i32FrameFmt = VIDEO_PALETTE_YUV420P;

		val = 1;
		if (ioctl(i32VidFD, VIDIOCCAPTURE, &val) < 0)
		{
			printf("VIDIOCCAPTURE: %s\n", strerror(errno));
			err = ERR_AVDEV_VID_CAPTURE;
			goto fail;
		}

		s_sVidData.i32UseMMap = 0;
	} 
	else
	{
		s_sVidData.pu8VidBuf = (uint8_t *)mmap(0, s_sVidData.sVidMBufs.size, PROT_READ|PROT_WRITE, MAP_SHARED, i32VidFD, 0);
		if ((unsigned char*)-1 == s_sVidData.pu8VidBuf)
		{
			s_sVidData.pu8VidBuf = (uint8_t *)mmap(0, s_sVidData.sVidMBufs.size, PROT_READ|PROT_WRITE, MAP_PRIVATE, i32VidFD, 0);
			if ((unsigned char*)-1 == s_sVidData.pu8VidBuf)
			{
				printf("mmap: %s\n", strerror(errno));
				err = ERR_V4L_MMAP;
				goto fail;
			}
		}
		s_sVidData.i32VidFrame = 0;

		/* start to grab the first frame */
		s_sVidData.sVidMMap.frame = s_sVidData.i32VidFrame % s_sVidData.sVidMBufs.frames;
		s_sVidData.sVidMMap.height = i32Height;
		s_sVidData.sVidMMap.width = i32Width;
		s_sVidData.sVidMMap.format = VIDEO_PALETTE_YUV420P;

		if (ioctl(i32VidFD, VIDIOCMCAPTURE, &s_sVidData.sVidMMap) < 0)
		{
			if (errno != EAGAIN)
			{
				printf("VIDIOCMCAPTURE: %s\n", strerror(errno));
			} 
			else
			{
				printf("Fatal: grab device does not receive any video signal\n");
			}
			err = ERR_V4L_VID_MCAPTURE;
			goto fail;
		}
		s_sVidData.i32FrameFmt = s_sVidData.sVidMMap.format;
		s_sVidData.i32UseMMap = 1;
	}

	s_sVidData.i32FrameSize = s_sVidData.sVidMMap.width  * s_sVidData.sVidMMap.height* 2;
	s_sVidData.i32VidFD = i32VidFD;
	return err;
fail:
	if (i32VidFD >= 0)	
	{
		close(i32VidFD);
	}
	return err;
	
}

ERRCODE camera_v4l::ReadV4LPicture( uint8_t **ppu8PicPtr,uint64_t *pu64TimeStamp,uint32_t *pu32BufPhyAddr)
{
	int32_t i32TryCnt = 0;
	unsigned char *pchVidInBuf;
	struct v4l2_buffer sV4L2Buff;
	
	*ppu8PicPtr = NULL;

	while (ioctl(s_sVidData.i32VidFD, VIDIOCSYNC, &s_sVidData.i32VidFrame) < 0 &&
		(errno == EAGAIN || errno == EINTR))
	{
		usleep(10000);
		i32TryCnt ++;
		if(i32TryCnt >= 100)
		{
			printf(": V4L fail\n");
			return ERR_V4L_VID_SYNC;
		}
	}

	sV4L2Buff.index = s_sVidData.i32VidFrame;
	ioctl(s_sVidData.i32VidFD, VIDIOCGCAPTIME, &sV4L2Buff);
	*pu64TimeStamp = (uint64_t)sV4L2Buff.timestamp.tv_sec * 1000000 + (uint64_t)sV4L2Buff.timestamp.tv_usec;
	*pu32BufPhyAddr = sV4L2Buff.m.userptr;	/* encode physical address */
	
	if(s_u64PrevTS != 0)
	{
		if((*pu64TimeStamp - s_u64PrevTS) > 300000)
		{
#ifdef DEBUG_CF
			printf(": V4L get raw picture over %ld us\n", (*pu64TimeStamp - s_u64PrevTS));	
#endif
		}
	}
	s_u64PrevTS = *pu64TimeStamp;		
		
	pchVidInBuf = s_sVidData.pu8VidBuf + s_sVidData.sVidMBufs.offsets[s_sVidData.i32VidFrame];

	*ppu8PicPtr = pchVidInBuf;
	

#if defined (STATISTIC)

	if(s_u32VidInFrames == 0){
		s_u32VidInFirstTS = s_u64PrevTS;
	}

	s_u32VidInFrames ++;
	
	if(s_u32VidInFrames == 300){
#ifdef DEBUG_CF		
		printf(": Vin frame rate %ld fps \n", s_u32VidInFrames/((s_u64PrevTS - s_u32VidInFirstTS)/1000000)); 
#endif
		s_u32VidInFrames = 0;
	}
#endif

	return ERR_V4L_SUCCESS;

}

ERRCODE camera_v4l::TriggerV4LNextFrame(void)
{
	int32_t i32TryCnt = 0;

	while (ioctl(s_sVidData.i32VidFD, VIDIOCSYNC, &s_sVidData.i32VidFrame) < 0 &&
		(errno == EAGAIN || errno == EINTR))
	{
		usleep(10000);
		i32TryCnt ++;
		if(i32TryCnt >= 100)
		{
			printf("TriggerV4LNextFrame: V4L fail\n");
			return ERR_V4L_VID_SYNC;
		}
	}

	s_sVidData.sVidMMap.frame = s_sVidData.i32VidFrame;
	if (ioctl(s_sVidData.i32VidFD, VIDIOCMCAPTURE, &s_sVidData.sVidMMap) < 0)
	{
		if (errno == EAGAIN)
			printf(": Cannot Sync\n");
		else
			printf(": VIDIOCMCAPTURE %s\n", strerror(errno));
		return ERR_V4L_VID_MCAPTURE;
	}

	/* This is now the grabbing frame */
	s_sVidData.i32VidFrame = (s_sVidData.i32VidFrame + 1) % s_sVidData.sVidMBufs.frames;
	return ERR_V4L_SUCCESS;

}

void  camera_v4l::FinializeV4LDevice()
{
	if (s_sVidData.i32UseMMap)
		munmap(s_sVidData.pu8VidBuf, s_sVidData.sVidMBufs.size);
	close(s_sVidData.i32VidFD);
	s_sVidData.i32VidFD = -1;

}

ERRCODE camera_v4l::StartV4LCapture(void)
{
	uint32_t u32Capture = VIDEO_START;
	printf("User Start Capture\n");
	if (ioctl(s_sVidData.i32VidFD, VIDIOCCAPTURE, &u32Capture) < 0)
	{
		return ERR_V4L_VID_CAPTURE;
	}
	s_sVidData.i32VidFrame = 0;
	return ERR_V4L_SUCCESS;


}

ERRCODE camera_v4l::StopV4LCapture(void)
{
	uint32_t u32Capture = VIDEO_STOP;
	if (ioctl(s_sVidData.i32VidFD, VIDIOCCAPTURE, &u32Capture) < 0) {
		return ERR_V4L_VID_CAPTURE;
	}
	return ERR_V4L_SUCCESS;
}

ERRCODE camera_v4l::SetV4LViewWindow(int lcmwidth, int lcmheight, int prewidth, int preheight)
{

	ERRCODE err = ERR_V4L_SUCCESS;
	s_sVidData.sVidWin.width = prewidth;
	s_sVidData.sVidWin.height = preheight;
	s_sVidData.sVidWin.x = (lcmwidth-prewidth)/2;		//Depend on the VPOST 
	s_sVidData.sVidWin.y = (lcmheight-preheight)/2;		
	s_sVidData.sVidWin.chromakey = -1;
	s_sVidData.sVidWin.flags = 0;

	printf("lcmwidth = %d\n lcmheight = %d\n prewidth = %d\n preheight = %d\n",lcmwidth,lcmheight,prewidth,preheight);

	
	printf("User Packet View Win Width , Height = %d, %d\n", s_sVidData.sVidWin.width, s_sVidData.sVidWin.height);
	printf("User Packet View Win Position X , Y = %d, %d\n", s_sVidData.sVidWin.x, s_sVidData.sVidWin.y);

	if (ioctl(s_sVidData.i32VidFD, VIDIOCSWIN, &s_sVidData.sVidWin) < 0)
	{
		printf("VIDIOCSWIN: %s\n", strerror(errno));
		printf("User VIDIOCSWIN error\n");
		err = ERR_V4L_VID_SET_WIN;
		goto fail;
	}
	return err;
fail:
	if (s_sVidData.i32VidFD >= 0)
		close(s_sVidData.i32VidFD);
	return err;


}
ERRCODE camera_v4l::SetV4LEncode(int frame, int width, int height, int palette)
{
	ERRCODE err = ERR_V4L_SUCCESS;
	s_sVidData.sVidMMap.frame =  frame; //s_sVidData.i32VidFrame;
	s_sVidData.sVidMMap.format = palette; //VIDEO_PALETTE_YUV422P or VIDEO_PALETTE_YUV420P
	s_sVidData.sVidMMap.height = height;
	s_sVidData.sVidMMap.width =  width;
	s_sVidData.i32FrameSize = s_sVidData.sVidMMap.width  * s_sVidData.sVidMMap.height* 2;
	printf("\n");
	
	printf("\n");
	printf("\n");

	printf("s_sVidData.sVidMMap.height = %d\n   s_sVidData.sVidMMap.width = %d\n",s_sVidData.sVidMMap.height,s_sVidData.sVidMMap.width);
	printf("\n");
	printf("\n");

	if (ioctl(s_sVidData.i32VidFD, VIDIOCMCAPTURE, &s_sVidData.sVidMMap) < 0) {
		if (errno == EAGAIN)
			printf(": Cannot Sync\n");
		else
			printf(": VIDIOCMCAPTURE %s\n", strerror(errno));
		return ERR_V4L_VID_MCAPTURE;
	}
	return err;
}

ERRCODE camera_v4l::StartPreview(void)
{
	if (ioctl(s_sVidData.i32VidFD, VIDIOCSPREVIEW, 1) < 0) 
	{
		return ERR_V4L_VID_CAPTURE;
	}
	return ERR_V4L_SUCCESS;

}
ERRCODE camera_v4l::StopPreview(void)
{
	if (ioctl(s_sVidData.i32VidFD, VIDIOCSPREVIEW, 0) < 0) 
	{
		return ERR_V4L_VID_CAPTURE;
	}
	return ERR_V4L_SUCCESS;
}
ERRCODE camera_v4l::QueryV4LUserControl(void)
{
	struct v4l2_queryctrl queryctrl;
	ERRCODE err = ERR_V4L_SUCCESS;
	unsigned int id;

	memset (&queryctrl, 0, sizeof (queryctrl));
	printf ("QueryV4LUserControl standard start \n");
	for (id = V4L2_CID_BASE;
		id < V4L2_CID_LASTP1;
		id=id+1) 
	{
			queryctrl.id =id;
			printf ("QueryV4LUserControl %d \n", queryctrl.id);
			if (0 == ioctl (s_sVidData.i32VidFD, VIDIOC_QUERYCTRL, &queryctrl))
			{
				if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
				{
					printf ("V4L2_CTRL_FLAG_DISABLED %d \n", queryctrl.id);
					continue;
				}
			}
			else
			{
				if (errno == EINVAL)
				{
					printf ("EINVAL %d \n", queryctrl.id);
					continue;
				}	
				printf ("VIDIOC_QUERYCTRL");
				err = ERR_V4L_VID_QUERY_UID;	
			}
	}
	printf ("QueryV4LUserControl private start \n");
	for (id = V4L2_CID_PRIVATE_BASE; ;id=id+1)
	{	
			queryctrl.id =id;
			if (0 == ioctl (s_sVidData.i32VidFD, VIDIOC_QUERYCTRL, &queryctrl))
			{
				if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
					continue;	
				if(ctrl_id>MAX_USER_CTRL)
				{
					err = ERR_V4L_VID_QUERY_UID;	
					break;
				}					
				memcpy(&PrivateUserCtrl[ctrl_id], &queryctrl, sizeof(queryctrl)); 
				printf ("********************************************\n");
				printf ("id %d\n", PrivateUserCtrl[ctrl_id].id);
				printf ("name %s\n", PrivateUserCtrl[ctrl_id].name);
				printf ("minimum %d\n", PrivateUserCtrl[ctrl_id].minimum);
				printf ("maximum %d\n", PrivateUserCtrl[ctrl_id].maximum);	
				printf ("step %d\n", PrivateUserCtrl[ctrl_id].step);	
				ctrl_id = ctrl_id +1;
				
			}
			else 
			{
				if (errno == EINVAL)
					break;
				printf("VIDIOC_QUERYCTRL");
				err = ERR_V4L_VID_QUERY_UID;	
			}
	}
	return err;

}
struct v4l2_queryctrl* camera_v4l::ctrl_by_name(const char* name)
{
	unsigned int i;

	for (i = 0; i < MAX_USER_CTRL; i++)
	{
		if( strcmp((char*)PrivateUserCtrl[i].name, name)==0 )
		{
			return PrivateUserCtrl+i;
		}
	}
	return NULL;
}

ERRCODE camera_v4l::ChangeV4LUserControl_Brigness(int32_t ctrl)
{
	ERRCODE err = ERR_V4L_SUCCESS;
	struct v4l2_control control;
	struct v4l2_queryctrl* 		i2c_set_addr_ctrl;
	struct v4l2_queryctrl* 		i2c_write;
	struct v4l2_queryctrl* 		i2c_read;
	printf("!!!Remmember that the function only works for OV7725 Sesnor\n");
	
	i2c_set_addr_ctrl = ctrl_by_name("i2c_set_addr");
	i2c_write = ctrl_by_name("i2c_write");
	i2c_read= ctrl_by_name("i2c_read");
	
	/* Enable Brightness Adjustment */
	control.id = i2c_set_addr_ctrl->id;
	control.value = REG_SDE_CTRL;
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CTRL, &control)) 
	{//Set I2C Reg Addr
		printf("VIDIOC_G_CTRL fail\n");
		return -1; 
	}
	control.id = i2c_read->id;
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_G_CTRL, &control)) 
	{//Read back 
		printf("VIDIOC_G_CTRL fail\n");
		return -1; 
	}
	control.id = i2c_write->id;
	control.value = control.value | 0x04;
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CTRL, &control)) 
	{//Enable brightness/constrast fail
		printf("VIDIOC_S_CTRL fail\n");
		return -1; 
	}

	/*  Brightness Ajustment */
	control.id = i2c_set_addr_ctrl->id;
	control.value =  REG_BRIGHT;
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CTRL, &control))
	{//Set I2C Reg Addr
		printf("VIDIOC_G_CTRL fail\n");
		return -1; 
	}
	control.id = i2c_read->id;
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_G_CTRL, &control)) 
	{
		printf("VIDIOC_G_CTRL fail\n");
		return -1; 
	}
	control.id = i2c_write->id;	
	if(ctrl>=0)
	{	
		control.value = control.value+1;
		if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CTRL, &control)) 
		{//Enable brightness/constrast fail 
			printf("VIDIOC_S_CTRL fail\n");
			return -1; 
		}
	}
	else
	{
		control.value = control.value-1;
		if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CTRL, &control)) 
		{//Enable brightness/constrast fail 
			printf("VIDIOC_S_CTRL fail\n");
			return -1; 
		}
	}	
	return err;		
}
ERRCODE camera_v4l::QueryV4LZoomInfo(struct v4l2_cropcap *psVideoCropCap,struct v4l2_crop *psVideoCrop)
{
	
	ERRCODE err = ERR_V4L_SUCCESS;

	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_CROPCAP, psVideoCropCap))
	{//Get cropping window capacity 
		printf("VIDIOC_CROPCAP fail\n");
		return -1; 
	}
	
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_G_CROP, psVideoCrop)) 
	{//Get current cropping window
			printf("VIDIOC_G_CROP fail\n");
			return -1; 
	}
	return err;
}
ERRCODE camera_v4l::QueryV4LSensorID(int32_t* pi32SensorId)
{
	
	ERRCODE err = ERR_V4L_SUCCESS;

	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_QUERY_SENSOR_ID, pi32SensorId)) 
	{//Get cropping window capacity 
		printf("VIDIOC_QUERY_SENSOR_ID fail\n");
		return -1; 
	}	
	return err;
}

ERRCODE camera_v4l::Zooming(struct v4l2_cropcap *psVideoCropCap,struct v4l2_crop *psVideoCrop,int32_t ctrl)
{
	
	ERRCODE err = ERR_V4L_SUCCESS;

	if (QueryV4LZoomInfo(psVideoCropCap, psVideoCrop)!=ERR_V4L_SUCCESS)
	{
		printf("Query zoom info fail\n");
			return -1; 	
	}

	if(ctrl==1)
	{//zoom in 				
		if( ((psVideoCrop->c.width - psVideoCropCap->pixelaspect.denominator) >= psVideoCropCap->defrect.width/8) &&
		     ((psVideoCrop->c.height - psVideoCropCap->pixelaspect.numerator) >= psVideoCropCap->defrect.height/8) )
		{//Limits the JPEG upscale to 8X.
			printf("Zooming in\n");	
			psVideoCrop->c.width = psVideoCrop->c.width - psVideoCropCap->pixelaspect.denominator;
			psVideoCrop->c.height = psVideoCrop->c.height - psVideoCropCap->pixelaspect.numerator;
			psVideoCrop->c.left = psVideoCrop->c.left + psVideoCropCap->pixelaspect.denominator/2;
			psVideoCrop->c.top = psVideoCrop->c.top + psVideoCropCap->pixelaspect.numerator/2;
			if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CROP, psVideoCrop)) {//Set current cropping window
				printf("VIDIOC_S_CROP fail\n");
				return -1; 
			}	
		}		
	}
	else 
	{//zoom out
		if( ((psVideoCrop->c.width + psVideoCropCap->pixelaspect.denominator) <= psVideoCropCap->defrect.width) &&
		     ((psVideoCrop->c.height + psVideoCropCap->pixelaspect.numerator) <= psVideoCropCap->defrect.height) ) 
		{//cropping dimension must be less or equal defrect. 
			printf("Zooming out\n");
			psVideoCrop->c.width = psVideoCrop->c.width + psVideoCropCap->pixelaspect.denominator;
			psVideoCrop->c.height = psVideoCrop->c.height + psVideoCropCap->pixelaspect.numerator;
			psVideoCrop->c.left = psVideoCrop->c.left - psVideoCropCap->pixelaspect.denominator/2;
			psVideoCrop->c.top = psVideoCrop->c.top - psVideoCropCap->pixelaspect.numerator/2;
			if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_S_CROP, psVideoCrop)) {//Set current cropping window
				printf("VIDIOC_S_CROP fail\n");
				return -1; 
			}	
		}		
	}
	printf("cropcap.defrect.left =%d\n",psVideoCropCap->defrect.left);			/* default window information */
	printf("cropcap.defrect.top =%d\n",psVideoCropCap->defrect.top);
	printf("cropcap.defrect.width =%d\n",psVideoCropCap->defrect.width);
	printf("cropcap.defrect.height =%d\n",psVideoCropCap->defrect.height);
	printf("cropcap.pixelaspect.numerator =%d\n",psVideoCropCap->pixelaspect.numerator);	/* constraint for step in y direct */
	printf("cropcap.pixelaspect.denominator =%d\n",psVideoCropCap->pixelaspect.denominator);/* constraint for step in x direct */

	printf("crop.c.left =%d\n",psVideoCrop->c.left);
	printf("crop.c.top =%d\n",psVideoCrop->c.top);
	printf("crop.c.width =%d\n",psVideoCrop->c.width);
	printf("crop.c.height =%d\n",psVideoCrop->c.height);
	
	return err;
}
ERRCODE  camera_v4l::MotionDetV4LSetThreshold(uint32_t u32Threshold)
{	
return 0;
}
ERRCODE camera_v4l::MotionDetV4LDumpDiffBuffer(void)
{
	return 0;
}

ERRCODE camera_v4l::QueryV4LPacketInformation(S_PIPE_INFO* ps_packetInfo)
{
	ERRCODE err = ERR_V4L_SUCCESS;

	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_G_PACKET_INFO, ps_packetInfo)) 
	{//Get packet offset releate to s_sVidData.sVidMBufs
		printf("VIDIOC_G_PACKET_INFO fail\n");
		return -1; 
	}
	return err;

}

ERRCODE camera_v4l::QueryV4LPlanarInformation(S_PIPE_INFO* ps_planarInfo)
{
	ERRCODE err = ERR_V4L_SUCCESS;
	
	if (0 != ioctl (s_sVidData.i32VidFD, VIDIOC_G_PLANAR_INFO, ps_planarInfo)) 
	{//Get packet offset releate to s_sVidData.sVidMBufs
		printf("VIDIOC_G_PACKET_INFO fail\n");
		return -1; 
	}
	return err;
}
int32_t camera_v4l::ChangeEncodeV4LResoultion(int32_t* pi32_width, int32_t* pi32_height)
{
	int32_t i32width, i32height;
	int32_t err = 0;
	int val;

	/* Stop capture */
	val = 0;
	if (ioctl(s_sVidData.i32VidFD, VIDIOCCAPTURE, &val) < 0) 
	{
		printf("VIDIOCCAPTURE: %s\n", strerror(errno));
		err = ERR_AVDEV_VID_CAPTURE;
		goto fail;
	}

#if 0
	printf("Please input the sensor output dimension\n");
	printf("For example, NT99141 support 640x480, 800x600, and 1280x720\n");
	printf("Please input the sensor output width\n");
	scanf("%d", &i32width);
	printf("Please input the sensor output height\n");
 	scanf("%d", &i32height);
	printf("input sensor dimension = %d x %d\n", i32width, i32height);
#else
	i32width = 320;
	i32height = 240;
#endif
#if 0	
	s_sVidData.sVidWin.width = i32width;
	s_sVidData.sVidWin.height = i32height;
	/* Change resolution */
	if (ioctl(s_sVidData.i32VidFD, VIDIOCSWIN, &s_sVidData.sVidWin) < 0) {
		printf("VIDIOCSWIN: %s\n", strerror(errno));
		err = ERR_V4L_VID_SET_WIN;
		goto fail;
	}
	s_sVidData.i32VidFrame = 0;			
#endif
	
	s_sVidData.sVidMMap.height = i32height;
	s_sVidData.sVidMMap.width = i32width; 
	s_sVidData.i32FrameSize = s_sVidData.sVidMMap.width  * s_sVidData.sVidMMap.height* 2;

	if (ioctl(s_sVidData.i32VidFD, VIDIOCMCAPTURE, &s_sVidData.sVidMMap) < 0) {
			if (errno != EAGAIN) {
				printf("VIDIOCMCAPTURE: %s\n", strerror(errno));
			} 
			else {
				printf("Fatal: grab device does not receive any video signal\n");
			}
			err = ERR_V4L_VID_MCAPTURE;
			goto fail;
		}


	/* Start capture */
	/* s_sVidData. frame need reset to 0 */
	s_sVidData.i32VidFrame = 0;
	if (ioctl(s_sVidData.i32VidFD, VIDIOCCAPTURE, &val) < 0) {
		printf("VIDIOCCAPTURE: %s\n", strerror(errno));
		err = ERR_AVDEV_VID_CAPTURE;
	}
	*pi32_width = i32width;
	*pi32_height = i32height;
fail:
	return err;
}

void camera_v4l::menuEncodeV4LResoultion(E_IMAGE_RESOL* peEncodeImgResol)
{
	int32_t i32width, i32height;
	ChangeEncodeV4LResoultion(&i32width, &i32height);	/* TBD */

	if(i32width==320)
		*peEncodeImgResol = eIMAGE_QVGA;
	else if(i32width==480)
		*peEncodeImgResol = eIMAGE_WQVGA;						
	else if(i32width==800)
		*peEncodeImgResol = eIMAGE_SVGA;
	else if(i32width==1280)
		*peEncodeImgResol = eIMAGE_HD720;
	else
		*peEncodeImgResol = eIMAGE_VGA;

	printf("change done\n");
}

void camera_v4l::menuChangeParameter(int i32FBFd,E_IMAGE_RESOL* pePreviewImgResol,E_IMAGE_RESOL* peEncodeImgResol)
{
	
	char item;
	printf("Change video paramenters\n");
	printf("[e] encode dimension\n");
	//printf("[s] preview dimension\n");
	printf("[p] disable preview (packet pipe is still on, doesn't show to LCM)\n"); 
	printf("[P] enable preview (packet pipe is still on, show to LCM)\n");
#if 0	
	fflush(stdin);
	scanf("%c", &item); 
#else
	item = 'e';
	
#endif
	switch(item)
	{
		//case 's': printf("change preview width and height\n");
			//menupreviewV4LResoultion(pePreviewImgResol);
			//break;
		case 'e': printf("change encode width and height\n");
			menuEncodeV4LResoultion(peEncodeImgResol);		
			break;
		case 'p': printf("disable preview\n");
			StopPreview();
			ioctl(i32FBFd, IOCTL_LCD_DISABLE_INT, 0);	
			break;
		case 'P': printf("enable preview\n");
			StartPreview();
			ioctl(i32FBFd, IOCTL_LCD_ENABLE_INT, 0);
			break;
		default:
			break;
	}
}
int camera_v4l::dumpV4LBuffer(void)
{
	int32_t val;
	unsigned char *pVideoBuf;
	#ifdef __DUMP_PACKET__
		S_PIPE_INFO s_packetInfo;
	#else
		S_PIPE_INFO s_planarInfo;
	#endif
		int file;
		
	
	#ifdef __DUMP_PACKET__
		char szFileName[]="DumpPacket.pb";
		QueryV4LPacketInformation(&s_packetInfo);
		//mapping Packet 0 (start from 3) 
		val = 3;
		printf("Packet buffer no = 0x%x\n", s_packetInfo.i32PipeBufNo);
		printf("Packet buffer memory size = 0x%x\n", s_packetInfo.i32PipeBufSize);
		printf("Current Packet buffer physical address = 0x%x\n", s_packetInfo.i32CurrPipePhyAddr);
	#else
		char szFileName[]="DumpPlanar.pb";
		QueryV4LPlanarInformation(&s_planarInfo);
		//mapping planar 0. Packet start from 3. 
		val = 0;
		printf("Planar buffer no = 0x%x\n", s_planarInfo.i32PipeBufNo);
		printf("Planar buffer memory size = 0x%x\n", s_planarInfo.i32PipeBufSize);
		printf("Current Planar buffer physical address = 0x%x\n", s_planarInfo.i32CurrPipePhyAddr);
	#endif
		
		if (ioctl(s_sVidData.i32VidFD, VIDIOC_S_MAP_BUF, &val) < 0) {
			printf("VIDIOC_S_MAP_BUF: %s\n", strerror(errno));
			return -1;
		}
	
	
	#ifdef __DUMP_PACKET__
		pVideoBuf = (unsigned char *)mmap(NULL, s_packetInfo.i32PipeBufSize, PROT_READ|PROT_WRITE, MAP_SHARED, s_sVidData.i32VidFD, 0);
	#else
		pVideoBuf = (unsigned char *)mmap(NULL, s_planarInfo.i32PipeBufSize, PROT_READ|PROT_WRITE, MAP_SHARED, s_sVidData.i32VidFD, 0);
	#endif
		file = open(szFileName, O_CREAT | O_WRONLY);
		if(file != -1){
			printf("file is created. \n");
		}	 
		else
			return -1;
	#ifdef __DUMP_PACKET__
		val = write(file, pVideoBuf, (320*240*2));
	#else
		val = write(file, pVideoBuf, (640*480*2));
	#endif
		if(val == -1){
			printf("file write error. \n");
			return -1;
		}else {
			printf("file write %d byte. \n", val);
		}	
		close(file);
	#ifdef __DUMP_PACKET__
		munmap(pVideoBuf, s_packetInfo.i32PipeBufSize);
	#else
		munmap(pVideoBuf, s_planarInfo.i32PipeBufSize);
	#endif
		printf("file is write. \n");
		return 0;

}

#if 0
int main()
{
	E_IMAGE_RESOL ePreviewImgResol = eIMAGE_QVGA;	
	E_IMAGE_RESOL eEncodeImgResol = eIMAGE_VGA;

	camera_v4l v4l_device("/dev/video0");

	v4l_device.InitV4LDevice(eEncodeImgResol);
	return 0;
}
#endif
